import sys
sys.path.append('/home/pi/MasterPi')
import cv2
import Functions.Car_demo as Cardemo
import HiwonderSDK.Sonar as Sonar
import apriltag
import math
import signal
import time
import os
import numpy as np
import threading

# 全局变量
# 超声波
SONAR=Sonar.Sonar()
# 摄像头
CAP=cv2.VideoCapture('http://127.0.0.1:8080?action=stream')
# 二维码检测器
apriltag_path = '/home/pi/.local/lib/python3.7/site-packages/libapriltag.so'
searchpath = [os.path.dirname(apriltag_path)]
detector = apriltag.Detector(searchpath=searchpath)
# 检测到的二维码
detect_id=None
# 动作组
action_group={0:"r",1:"r",2:"l",3:"mr",4:'s',5:"s"}

center=False

__isRunning=False
size_m = (320, 240)

def Init():
    print('Init')
    # 初始化机械臂位置
    Cardemo.set_first_Servo(1000,1500)
    Cardemo.set_second_Servo(1000,700)
    Cardemo.set_third_Servo(1000,2200)
    Cardemo.set_fourth_Servo(1000,1000)

def Start():
    print('Start')
    # 启动小车
    Cardemo.Forward()

    global __isRunning
    __isRunning=True

def Stop(signum, frame):
    print('Stop')
    # 停止小车
    Cardemo.Stop()
    global __isRunning
    __isRunning=False

def detect(img):
    # 用摄像头识别二维码
    global detector
    global size_m
    global center
    img_copy = img.copy()
    img_h, img_w = img.shape[:2]
    frame_resize = cv2.resize(img_copy, size_m, interpolation=cv2.INTER_NEAREST)
    gray = cv2.cvtColor(frame_resize, cv2.COLOR_BGR2GRAY)
    detections = detector.detect(gray, return_image=False)
    tag_id = None

    target_x = img_w // 2


    def map(x, in_min, in_max, out_min, out_max):
        return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min

    if len(detections) != 0:
        for i, detection in enumerate(detections):
            tag_id = int(detection.tag_id)  # 获取tag_id
            corners = np.rint(detection.corners)  # 获取四个角点
            for i in range(4):
                corners[i][0] = int(map(corners[i][0], 0, size_m[0], 0, img_w))
                corners[i][1] = int(map(corners[i][1], 0, size_m[1], 0, img_h))
            cv2.drawContours(img, [np.array(corners, np.int32)], -1, (0, 255, 255), 2)
            object_center_x = int(map(detection.center[0], 0, size_m[0], 0, img_w))
            object_center_y = int(map(detection.center[1], 0, size_m[1], 0, img_h))  # 中心点
            object_angle = int(math.degrees(math.atan2(corners[0][1] - corners[1][1], corners[0][0] - corners[1][0])))  # 计算旋转角
            cv2.putText(img, str(tag_id), (object_center_x - 10, object_center_y + 10), cv2.FONT_HERSHEY_SIMPLEX, 1,
                        [0, 255, 255], 2)
            
            offset_x = object_center_x - target_x
            if (offset_x > 30):
                center = False
            elif (offset_x < -30):
                center = False
            else:
                center = True
            offset_x = object_center_x - target_x
            print(offset_x)

    if tag_id!=None:
        global detect_id
        detect_id = tag_id

    return tag_id,img

def turn_dec():
    global center
    global detect_id
    global __isRunning
    while True:
        dist=SONAR.getDistance()
        print(f'dist:{dist}')
        if dist <= 300:
            Cardemo.Stop()
            time.sleep(0.1)
            orient=action_group[detect_id]
            if orient=='l':
                Cardemo.arm_both()
                time.sleep(1.5)
                Cardemo.turn_left()
                time.sleep(0.2)
                while not center:
                    time.sleep(0.1)
                    Cardemo.Stop()
                    time.sleep(0.5)
                    Cardemo.turn_left()
                Cardemo.Stop()
            elif orient=='r':
                Cardemo.arm_both()
                time.sleep(1.5)
                Cardemo.turn_right()
                time.sleep(0.2)
                while not center:
                    time.sleep(0.1)
                    Cardemo.Stop()
                    time.sleep(0.5)
                    Cardemo.turn_right()
                Cardemo.Stop()
            elif orient=='mr':
                Cardemo.arm_both()
                time.sleep(1.5)
                Cardemo.Right()
                while not center:
                    time.sleep(0.1)
                Cardemo.Stop()
                Cardemo.Forward()

            elif orient=='s' or (not __isRunning):
                Cardemo.Stop()
                Cardemo.arm_up()
                __isRunning=False
                break
            else:
                continue
            Cardemo.Forward()
        else:
            time.sleep(0.03)
TD=threading.Thread(target=turn_dec)
TD.daemon = True
TD.start()

if __name__=='__main__':
    Init()
    Start()
    signal.signal(signal.SIGINT,Stop)
    while __isRunning:
        ret,img=CAP.read()
        if ret:
            tag_id,frame=detect(img)
            frame_resize = cv2.resize(frame, (320, 240))
            cv2.imshow('frame', frame_resize)
            key = cv2.waitKey(1)
            if key == 27:
                break
        else:
            time.sleep(0.01)
    cv2.destroyAllWindows()